/**************************************************************************************************
* Title: SumVector4_16f.cpp
* Author: Gael Huber
* Description: Implements 16-byte aligned 4D vectors
**************************************************************************************************/
#include "SumMath.h"

/**************************************************************************************************
* Cross product of a 4D vector
**************************************************************************************************/
Vector4_16f* Vec4Cross_16f(Vector4_16f* pOut, const Vector4_16f* pV1, const Vector4_16f* pV2, 
	const Vector4_16f* pV3) {
	pOut->x = pV1->y * (pV2->z * pV3->w - pV2->w * pV3->z)
			- pV1->z * (pV2->y * pV3->w - pV2->w * pV3->y)
			+ pV1->w * (pV2->y * pV3->z - pV2->z * pV3->y);

	pOut->y = pV1->x * (pV2->w * pV3->z - pV2->z * pV3->w)
			- pV1->z * (pV2->w * pV3->x - pV2->x * pV3->w)
			+ pV1->w * (pV2->z * pV3->x - pV2->x * pV3->z);

	pOut->z = pV1->x * (pV2->y * pV3->w - pV2->w * pV3->y)
			- pV1->y * (pV2->x * pV3->w - pV2->w * pV3->x)
			+ pV1->w * (pV2->x * pV3->y - pV2->y * pV3->x);

	pOut->w = pV1->x * (pV2->z * pV3->y - pV2->y * pV3->z)
			- pV1->y * (pV2->z * pV3->x - pV2->x * pV3->z)
			+ pV1->z * (pV2->y * pV3->x - pV2->x * pV3->y);	
	return pOut;
}

/**************************************************************************************************
* Normalize
**************************************************************************************************/
Vector4_16f* Vec4Normalize_16f(Vector4_16f* pOut, const Vector4_16f* pV) {
	const float f(1.0f / Vec4Length_16f(pV));
	__m128 l = _mm_load1_ps(&f);
	pOut->v = _mm_mul_ps(pV->v, l);
	return pOut;
}

/**************************************************************************************************
* Hermite interpolation between position V1, tangent T1 (when s == 0) and position V2, tangent T2 
* (when s == 1).
**************************************************************************************************/
Vector4_16f* Vec4Hermite_16f(Vector4_16f* pOut, const Vector4_16f* pV1, const Vector4_16f* pT1, 
	const Vector4_16f* pV2, const Vector4_16f* pT2, float s) {
	// Calculate a, b, c, and d values
	float a(2.0f*s*s*s - 3.0f*s*s + 1.0f);
	float b(-2.0f*s*s*s + 3.0f*s*s);
	float c(s*s*s - 2.0f*s*s + s);
	float d(s*s*s - s*s);

	// Solve for a*v1 + b*v2 + c*t1 + d*t2
	Vector4_16f v1, v2;

	Vec4Scale_16f(&v1, pV1, a);
	Vec4Scale_16f(&v2, pV2, b);
	Vec4Add_16f(pOut, &v1, &v2);

	Vec4Scale_16f(&v1, pT1, c);
	Vec4Add_16f(pOut, pOut, &v1);

	Vec4Scale_16f(&v1, pT2, d);
	Vec4Add_16f(pOut, pOut, &v1);

	return pOut;
}

/**************************************************************************************************
* CatmullRom interpolation between V1 (when s == 0) and V2 (when s == 1)
**************************************************************************************************/
Vector4_16f* Vec4CatmullRom_16f(Vector4_16f* pOut, const Vector4_16f* pV0, const Vector4_16f* pV1,
	const Vector4_16f* pV2, const Vector4_16f* pV3, float s) {
	// Calculate a, b, c, and d
	float a(-s*s*s + 2.0f*s*s - s);
	float b(3.0f*s*s*s - 5.0f*s*s + 2.0f);
	float c(-3.0f*s*s*s + 4.0f*s*s + s);
	float d(s*s*s - s*s);

	// Solve for (a*p1 + b*p2 + c*p3 + d*p4)/2
	Vector4_16f v1, v2;

	Vec4Scale_16f(&v1, pV0, a);
	Vec4Scale_16f(&v2, pV1, b);
	Vec4Add_16f(pOut, &v1, &v2);

	Vec4Scale_16f(&v1, pV2, c);
	Vec4Add_16f(pOut, pOut, &v1);

	Vec4Scale_16f(&v1, pV3, d);
	Vec4Add_16f(pOut, pOut, &v1);

	return pOut;
}

/**************************************************************************************************
* Barycentric coordinates V1 + f(V2 - V1) + g(V3 - V1)
**************************************************************************************************/
Vector4_16f* Vec4Barycentric_16f(Vector4_16f* pOut, const Vector4_16f* pV1, const Vector4_16f* pV2, const Vector4_16f* pV3,
	float f, float g) {
	pOut->v = _mm_add_ps(_mm_add_ps(pV1->v, 
		_mm_mul_ps(_mm_load1_ps(&f), _mm_sub_ps(pV2->v, pV1->v))), 
		_mm_mul_ps(_mm_load1_ps(&g), _mm_sub_ps(pV3->v, pV1->v))); 
	return pOut;
}

// Barycentric coordinates V1 + f(V2 - V1) + g(V3 - V1)
Vector4_16f* Vec4Barycentric_16f(Vector4_16f* pOut, const Vector4_16f* pV1, const Vector4_16f* pV2, const Vector4_16f* pV3,
	const __m128& f, const __m128& g) {
	pOut->v = _mm_add_ps(_mm_add_ps(pV1->v, 
		_mm_mul_ps(f, _mm_sub_ps(pV2->v, pV1->v))), 
		_mm_mul_ps(g, _mm_sub_ps(pV3->v, pV1->v))); 
	return pOut;
}

/**************************************************************************************************
* Transform by matrix (x, y, 0, 1)
**************************************************************************************************/
Vector4_16f* Vec4Transform_16f(Vector4_16f* pOut, const Vector4_16f* pV, const Matrix_16f* pM) {
	__m128 m(_mm_mul_ps(pV->v, pM->v[0]));
	pOut->x = m.m128_f32[0] + m.m128_f32[1] + m.m128_f32[2] + m.m128_f32[3];
	m = (_mm_mul_ps(pV->v, pM->v[1]));
	pOut->y = m.m128_f32[0] + m.m128_f32[1] + m.m128_f32[2] + m.m128_f32[3];
	m = (_mm_mul_ps(pV->v, pM->v[2]));
	pOut->z = m.m128_f32[0] + m.m128_f32[1] + m.m128_f32[2] + m.m128_f32[3];
	m = (_mm_mul_ps(pV->v, pM->v[3]));
	pOut->w = m.m128_f32[0] + m.m128_f32[1] + m.m128_f32[2] + m.m128_f32[3];
	return pOut;
}